package it.unibo.refolding.pico;

import it.unibo.refolding.common.CommonThings;
import it.unibo.refolding.common.DoubleIntPair;
import it.unibo.refolding.common.MyRuntimeException;
import it.unibo.refolding.pico.PicoUtil.Position;
import it.unibo.refolding.pico.stripchart.JpHelper;
import it.unimi.dsi.fastutil.doubles.DoubleArrayList;
import it.unimi.dsi.fastutil.doubles.DoubleList;
import it.unimi.dsi.fastutil.ints.IntArrayList;
import it.unimi.dsi.fastutil.ints.IntList;
import it.unimi.dsi.fastutil.ints.IntLists;

import java.io.IOException;
import java.io.UnsupportedEncodingException;
import java.nio.file.Files;
import java.nio.file.Path;
import java.util.Arrays;
import java.util.Locale;
import java.util.logging.Level;
import java.util.logging.Logger;
import java.util.regex.Matcher;
import java.util.regex.Pattern;

import org.apache.commons.lang3.ArrayUtils;
import org.apache.commons.lang3.builder.ReflectionToStringBuilder;
import org.apache.commons.lang3.builder.ToStringStyle;
import org.apache.commons.math3.stat.descriptive.rank.Percentile;

import com.google.common.base.Charsets;
import com.google.common.base.Preconditions;

public class PicoDataExtractor {

  private static final Logger logger = Logger.getLogger(PicoDataExtractor.class.getSimpleName());

  public static final byte[] endMarker = endMarker();

  static int headerLength(byte[] fileContent) {
    if (fileContent.length < endMarker.length) {
      logger.log(Level.WARNING, "Header is too short: fileContent.length={0}.", fileContent.length);
      return -1;
    }
    int i;
    for (i = 0; i <= fileContent.length - endMarker.length; ++i) {
      int j;
      for (j = 0; j < endMarker.length; ++j) {
        if (fileContent[i + j] != endMarker[j]) {
          break;
        }
      }
      if (j == endMarker.length) {
        break;
      }
    }
    if (i == fileContent.length - endMarker.length + 1) {
      logger.log(
        Level.WARNING, "Could not find header end for fileContent.length={0}.", fileContent.length);
      return -1;
    }
    int headerLength = i + endMarker.length;
    assert Arrays.equals(endMarker, Arrays.copyOfRange(fileContent, i, headerLength));
    return headerLength;
  }

  private static byte[] endMarker() {
    byte[] endMarker;
    try {
      endMarker = PicoUtil.FILE_LIST_END.getBytes("ISO-8859-1");
    } catch (UnsupportedEncodingException e1) {
      throw new MyRuntimeException(e1.getMessage(), e1);
    }
    return endMarker;
  }

  static String header(byte[] fileContent) {
    int headerLength = headerLength(fileContent);
    if (headerLength == -1) {
      return null;
    }
    return new String(fileContent, 0, headerLength, Charsets.ISO_8859_1);
  }

  private static final HeaderParserStrategy defaultHeaderParserStrategy = HeaderParserStrategy.JREGEX;

  private static final boolean REMOVE_Z_MINUS_ONE_DEFLECTION_ZERO_OR_MINUS_ONE = true;

  /**
   * Deflection against Time Z sensor against Time Deflection against Ch 2: Z
   * sensor
   */
  public static PicoData parse(byte[] fileContent) {
    return parse(fileContent, defaultHeaderParserStrategy);
  }

  public static PicoData parse(Path file, HeaderParserStrategy strategy) {
    byte[] fileContent;
    try {
      fileContent = Files.readAllBytes(file);
    } catch (IOException e) {
      throw new MyRuntimeException(e.getMessage(), e);
    }
    return parse(fileContent, strategy);
  }

  public static PicoData parse(byte[] fileContent, HeaderParserStrategy strategy) {
    if (fileContent == null) {
      return null;
    }
    String header = header(fileContent);
    if (header == null) {
      return null;
    }
    HeaderMatcher matcher = strategy.matcher(header);
    if (!matcher.matches()) {
      logger.log(
        Level.WARNING, "matcher did not match header for header.length()={0}.", header.length());
      return null;
    }
    int i = 0;
    String piezo = matcher.group(++i);
    double sensZSens = Double.parseDouble(matcher.group(++i));
    double xOffsetNm = Double.parseDouble(matcher.group(++i));
    String stripChartString = matcher.group(++i);
    boolean stripChart = stripChartString.equals("Enabled");
    assert stripChart || stripChartString.equals("Disabled");
    int stripChartRate = Integer.parseInt(matcher.group(++i));
    double sensDeflSensNmPerV = Double.parseDouble(matcher.group(++i));
    double sensZSensorSens = Double.parseDouble(matcher.group(++i));
    double deflectionLimit = Double.parseDouble(matcher.group(++i));
    double scanRate = Double.parseDouble(matcher.group(++i));
    double forwardVel = Double.parseDouble(matcher.group(++i));
    double reverseVel = Double.parseDouble(matcher.group(++i));
    double rampDelay = Double.parseDouble(matcher.group(++i));
    double zScanStartHardValueV = Double.parseDouble(matcher.group(++i));
    int dtDataOffset = Integer.parseInt(matcher.group(++i));
    int dtDataLength = Integer.parseInt(matcher.group(++i));
    double dtSpringConstant = Double.parseDouble(matcher.group(++i));
    int ztDataOffset = Integer.parseInt(matcher.group(++i));
    int ztDataLength = Integer.parseInt(matcher.group(++i));
    double ztSpringConstant = Double.parseDouble(matcher.group(++i));
    double ztSensZSensorSens = Double.parseDouble(matcher.group(++i));
    double ztRampSizeHardValue = Double.parseDouble(matcher.group(++i));
    assert i == matcher.groupCount();
    if (dtSpringConstant != ztSpringConstant) {
      logger.log(
        Level.WARNING, "Spring constants are not equal: dtSpringConstant={0} ztSpringConstant={1}",
        new Object[] {Double.toString(dtSpringConstant), Double.toString(ztSpringConstant)});
      return null;
    }
    if (dtDataOffset + dtDataLength > fileContent.length) {
      // Avoid a nasty ArrayIndexOutOfBoundsException.
      logger.log(
        Level.WARNING, "dtDataOffset={0} + dtDataLength={1} > fileContent.length={2}",
        new Object[] {dtDataOffset, dtDataLength, fileContent.length});
      return null;
    }
    short[] dtData = PicoUtil.extractLittleEndianData(fileContent, new Position(
      dtDataOffset, dtDataLength));
    if (ztDataOffset + ztDataLength > fileContent.length) {
      // Avoid a nasty ArrayIndexOutOfBoundsException.
      logger.log(
        Level.WARNING, "ztDataOffset={0} + ztDataLength={1} > fileContent.length={2}",
        new Object[] {dtDataOffset, ztDataLength, fileContent.length});
      return null;
    }
    short[] ztData = PicoUtil.extractLittleEndianData(fileContent, new Position(
      ztDataOffset, ztDataLength));
    if (dtData.length != ztData.length) {
      if (dtData.length < ztData.length) {
        boolean any = CommonThings.any(ztData, dtData.length, ztData.length);
        if (!any) {
          ztData = Arrays.copyOfRange(ztData, 0, dtData.length);
        } else {
          logger.log(
            Level.WARNING, "Data lengths are not equal: dtData.length={0} ztData.length={1}",
            new Object[] {dtData.length, ztData.length});
          return null;
        }
      } else {
        assert ztData.length < dtData.length;
        boolean any = CommonThings.any(dtData, ztData.length, dtData.length);
        if (!any) {
          dtData = Arrays.copyOfRange(dtData, 0, ztData.length);
        } else {
          logger.log(
            Level.WARNING, "Data lengths are not equal: dtData.length={0} ztData.length={1}",
            new Object[] {dtData.length, ztData.length});
          return null;
        }
      }
    }
    if (REMOVE_Z_MINUS_ONE_DEFLECTION_ZERO_OR_MINUS_ONE) {
      int k = 0;
      for (int j = 0; j < ztData.length; ++j) {
        // g4/unfolding/tris10mM_pH8_KCl95mM/20130529/05291640.104
        boolean toIgnore = (ztData[j] == 0 | ztData[j] == -1) & (dtData[j] == -1 | dtData[j] == 0);
        if (!toIgnore) {
          ztData[k] = ztData[j];
          dtData[k++] = dtData[j];
        }
      }
      if (k != ztData.length) {
        logger.log(Level.FINE, "Removed {0} points (z=-1, d=0).", ztData.length - k);
        ztData = Arrays.copyOf(ztData, k);
        dtData = Arrays.copyOf(dtData, k);
      }
    }
    double zLsbToMetresFactor = ztSensZSensorSens * sensZSensorSens / 1e9;
    short[] approachZ = stripChart ? ztData : Arrays.copyOfRange(ztData, 0, ztData.length / 2);
    if (logger.isLoggable(Level.FINER)) {
      int approachZFirstNonDecreasingIndex = CommonThings.firstNonDecreasingIndex(approachZ);
      if (approachZFirstNonDecreasingIndex != approachZ.length) {
        logger.log(Level.FINER, "approachZ[{0}]={1} approachZ[{2}]={3}", new Object[] {
        approachZFirstNonDecreasingIndex - 1, approachZ[approachZFirstNonDecreasingIndex - 1],
        approachZFirstNonDecreasingIndex, approachZ[approachZFirstNonDecreasingIndex]});
      }
    }
    // Why are both approach and retraction generally decreasing?
    short[] retractionZ = Arrays.copyOfRange(ztData, ztData.length / 2, ztData.length);
    if (logger.isLoggable(Level.FINER)) {
      int retractionZFirstNonDecreasingIndex = CommonThings.firstNonDecreasingIndex(retractionZ);
      if (retractionZFirstNonDecreasingIndex != retractionZ.length) {
        logger.log(Level.FINER, "retractionZ[{0}]={1} retractionZ[{2}]={3}", new Object[] {
        retractionZFirstNonDecreasingIndex - 1,
        retractionZ[retractionZFirstNonDecreasingIndex - 1], retractionZFirstNonDecreasingIndex,
        retractionZ[retractionZFirstNonDecreasingIndex]});
      }
    }
    final double deflectionSensitivityMPerV = sensDeflSensNmPerV / 1e9;
    final PicoData picoData;
    if (stripChart) {
      int deflectionZeroTail = CommonThings.countAtEnd(dtData, 0);
      int zZeroTail = CommonThings.countAtEnd(ztData, 0);
      if (deflectionZeroTail != zZeroTail) {
        logger.warning("Different zero tail sizes: deflectionZeroTail=" + deflectionZeroTail
          + " zZeroTail=" + zZeroTail);
      }
      int zeroTail = Math.min(deflectionZeroTail, zZeroTail);
      StripChartDirectionData approach = new StripChartDirectionData(Arrays.copyOfRange(
        dtData, 0, dtData.length - zeroTail), Arrays.copyOfRange(ztData, 0, ztData.length
        - zeroTail));
      picoData = new StripChartPicoData(
        xOffsetNm / 1e9, dtSpringConstant, sensZSens / 1e9, zScanStartHardValueV,
        zLsbToMetresFactor, deflectionLimit, deflectionSensitivityMPerV, approach, stripChartRate);
    } else {
      double velMultiplier = PicoConstants.VELOCITY_MULTIPLIERS.get(piezo);
      PlainDirectionData approach = new PlainDirectionData(Arrays.copyOfRange(
        dtData, 0, dtData.length / 2), Arrays.copyOfRange(ztData, 0, ztData.length / 2), forwardVel
        * velMultiplier);
      PlainDirectionData retraction = new PlainDirectionData(Arrays.copyOfRange(
        dtData, dtData.length / 2, dtData.length), Arrays.copyOfRange(
        ztData, ztData.length / 2, ztData.length), reverseVel * velMultiplier);
      if (!stripChart
        & (approachZ[0] <= approachZ[approachZ.length - 1] || retractionZ[0] <= retractionZ[retractionZ.length - 1])) {
        logger.log(Level.WARNING, "Both aproach and retraction should generally decrease.");
      }
      PlainPicoData plainPicoData = new PlainPicoData(
        xOffsetNm / 1e9, dtSpringConstant, sensZSens / 1e9, zScanStartHardValueV,
        zLsbToMetresFactor, deflectionLimit, deflectionSensitivityMPerV, scanRate, approach,
        retraction, ztRampSizeHardValue, rampDelay);
      double actualZApproachRangeM = plainPicoData.actualZApproachRangeMetres();
      double actualZApproachRangeMOverRampSizeM = actualZApproachRangeM
        / plainPicoData.rampSizeMetres();
      if (!stripChart
        && (actualZApproachRangeMOverRampSizeM < -1.25 | actualZApproachRangeMOverRampSizeM > -0.75)) {
        logger.warning(String.format(
          Locale.ROOT,
          "Ramp size and actual Z range are very different: actualZApproachRangeM=%s rampSize(nm)=%s actualZApproachRangeMOverRampSizeM=%s",
          actualZApproachRangeM, plainPicoData.rampSizeMetres() * 1e9,
          actualZApproachRangeMOverRampSizeM));
      }
      double actualZRetractionRangeM = plainPicoData.actualZRetractionRangeMetres();
      double actualZRetractionRangeMOverRampSizeM = actualZRetractionRangeM
        / plainPicoData.rampSizeMetres();
      if (actualZRetractionRangeMOverRampSizeM < -1.12
        | actualZRetractionRangeMOverRampSizeM > -0.88) {
        Level level = Level.FINE;
        if (logger.isLoggable(level)) {
          logger.log(level, String.format(
            "actualZRetractionRangeM=%s rampSizeMetres=%s", actualZRetractionRangeM,
            plainPicoData.rampSizeMetres()));
        }
      }
      picoData = plainPicoData;
    }
    return picoData;
  }

  public static double deflectionLsbToMetresFactor(double deflectionLimit, double sensDeflSensMPerV) {
    return deflectionLimit * sensDeflSensMPerV / 0xFFFF;
  }

  public static double sensDeflSensMPerV(double deflectionLimit, double deflectionLsbToMetresFactor) {
    return deflectionLsbToMetresFactor / deflectionLimit * 0xFFFF;
  }

  /**
   * @param stripChart
   * @param rampSizeM
   * @param surfaceDelayS
   * @param approachVelocityMOverS
   * @param retractionVelocityMOverS
   * @param iterations
   *          Nanoscope's loopCount + 1
   * @param extensiveRetraction
   * @return
   */
  static ApproachRetractionLoop stripChartToApproachRetraction(
    StripChartPicoData stripChart, LoopParams params, DoubleList allowedTimeRelativeError) {
    IntList junctionPoints = computeLoopJunctionPoints(
      stripChart, params, allowedTimeRelativeError);
    if (junctionPoints == null) {
      return null;
    }
    return createLoopGivenJunctionPoints(
      stripChart, params.approachRampSizeM, params.approachVelocityMPerS, params.retractionRampSizeM,
      params.retractionVelocityMPerS, params.iterations, junctionPoints);
  }

  private static int retractExtensivelyLastIteration(short[] z, int stripChartRateHz, int jp) {
    Preconditions.checkElementIndex(jp, z.length);
    /*
     * Dead time has 0.017 * 5000 = 85 data points. Check the lowest 5 ones.
     */
    Percentile percentile = new Percentile(5);
    // 000330_refold_false_adapter_02250259.001_95.386.pico wants 1.04
    double[] d = CommonThings.toDoubleArray(
      z, jp, Math.min(jp + (int) (1.04 * PicoConstants.DEAD_TIME_S * stripChartRateHz), z.length));
    double threshold = percentile.evaluate(d);
    int delta = CommonThings.indexOfFirstElementLessThanOrEqualTo(d, threshold);
    Preconditions.checkElementIndex(delta, d.length);
    return jp + delta;
  }

  public static IntList computeLoopJunctionPoints(
    StripChartPicoData stripChart, int from, int to, LoopParams params, DoubleList allowedTimeRelativeError) {
    IntList jp = computeLoopJunctionPoints(
      stripChart.subRange(from, to), params, allowedTimeRelativeError);
    if (jp == null) {
      return null;
    }
    int[] a = new int[jp.size()];
    for (int i = 0; i < jp.size(); ++i) {
      a[i] = jp.getInt(i) + from;
    }
    return IntLists.unmodifiable(IntArrayList.wrap(a));
  }
  
  public static IntList computeLoopJunctionPoints(
    StripChartPicoData stripChart, LoopParams params, DoubleList allowedTimeRelativeError) {
    IntList jp = computeLoopJunctionPoints(
      stripChart, params.approachRampSizeM, params.approachVelocityMPerS, params.surfaceDelayS, params.retractionRampSizeM,
      params.retractionVelocityMPerS, params.rectificationTimeS, params.iterations, allowedTimeRelativeError);
    if (params.extensiveRetraction & jp != null) {
      IntList a = new IntArrayList(jp);
      a.set(a.size() - 1, retractExtensivelyLastIteration(
        stripChart.sharedApproach().sharedZArray(), stripChart.stripChartRateHz, jp.getInt(jp.size() - 1)));
      return IntLists.unmodifiable(a);
    } else {
      return jp;
    }
  }

  /**
   * For empty segments, it does not impose any restriction on the slope.
   * 
   * @param stripChart
   * @param approachRampSizeM
   * @param approachVelocityMOverS
   * @param surfaceDelayS
   * @param retractionRampSizeM
   * @param retractionVelocityMOverS
   * @param iterations
   * @param allowedTimeRelativeError
   * @return
   */
  private static IntList computeLoopJunctionPoints(
    StripChartPicoData stripChart, double approachRampSizeM, double approachVelocityMOverS,
    double surfaceDelayS, double retractionRampSizeM, double retractionVelocityMOverS,
    double rectificationTimeS, int iterations, DoubleList allowedTimeRelativeError) {
    Preconditions.checkArgument(approachRampSizeM >= 0, approachRampSizeM);
    Preconditions.checkArgument(retractionRampSizeM >= 0, retractionRampSizeM);
    Preconditions.checkArgument(rectificationTimeS >= PicoConstants.DEAD_TIME_S);
    double approxTimeApproachPoints = approachRampSizeM / approachVelocityMOverS
      * stripChart.stripChartRateHz;
    approxTimeApproachPoints = Math.max(approxTimeApproachPoints,  1);
    double approxTimeRetractionPoints = retractionRampSizeM / retractionVelocityMOverS
      * stripChart.stripChartRateHz;
    approxTimeRetractionPoints = Math.max(approxTimeRetractionPoints, 1);
    double surfaceDelayPoints = surfaceDelayS * stripChart.stripChartRateHz;
    surfaceDelayPoints = Math.max(surfaceDelayPoints, 1);
    double deadTimePoints = PicoConstants.DEAD_TIME_S * stripChart.stripChartRateHz;
    // The first column type is Absolute.
//    double rectificationTimeS = Math.abs(approachRampSizeM - retractionRampSizeM) / PicoConstants.MAX_VELOCITY_M_PER_S;
//    double betweenTimePoints = (PicoConstants.DEAD_TIME_S + rectificationTimeS) * stripChart.stripChartRateHz;
    double betweenTimePoints = Math.max(rectificationTimeS * stripChart.stripChartRateHz,  1);
    double[] slopes = {
    0, speedInZLsbPerPoint(stripChart, approachVelocityMOverS),
    speedInZLsbPerPoint(stripChart, -retractionVelocityMOverS),};
    /*
     * In fact, there could be other 3 segments between one retraction and the next approach,
     * but for now they are just thrown together under rectificationTimeS without a slope
     * requirement when rectificationTimeS != PicoConstants.DEAD_TIME_S.
     */
    int k = ApproachRetractionLoop.APPROACH_RETRACTION_SEGMENTS + 1; // 4
    DoubleIntPair[] timeAndSlopeIndices = new DoubleIntPair[iterations * k - 1];
    /*
     * Although Nanoscope shows in the file a stretch of "doing nothing" at the
     * end from the captured file, I don't find it in the buffers.
     */
    for (int i = 0; i < iterations; ++i) {
      timeAndSlopeIndices[i * k] = new DoubleIntPair(
        approxTimeApproachPoints, approxTimeApproachPoints <= 1 ? -1 : 1);
      timeAndSlopeIndices[i * k + 1] = new DoubleIntPair(
        surfaceDelayPoints, surfaceDelayPoints <= 1 ? -1 : 0);
      timeAndSlopeIndices[i * k + 2] = new DoubleIntPair(
        approxTimeRetractionPoints, approxTimeRetractionPoints <= 1 ? -1 : 2);
      if (i != iterations - 1) {
        timeAndSlopeIndices[i * k + 3] = new DoubleIntPair(
          betweenTimePoints, rectificationTimeS != PicoConstants.DEAD_TIME_S ? -1 : 0);
      }
    }
    double[] atre = new double[timeAndSlopeIndices.length];
    Arrays.fill(atre, 0.04);
    for (int i = 0; i < iterations; ++i) {
      /*
       * 000090_refold_false_adapter_02250514.015_73.14.pico needs 0.06;
       * 000111_refold_false_adapter_02250515.016_73.986.pico needs 0.071;
       * 000150_refold_false_adapter_02250517.016_65.761.pico needs 0.085
       */
      // allowedTimeRelativeError[i * k ] = 0.085; // 0.06;
      
      /*
       * 000346_refold_true_adapter_02260321.009_110.pico wants a multiplication
       * factor of 0.14.
       * 000116_refold_true_117.00000000000003_adapter_02280224.006.pico wants
       * 0.144 000102_refold_true_152.99999999999997_adapter_02280449.009.pico
       * wants 0.22.
       * 000102_refold_true_152.99999999999997_adapter_02280449.009.pico wants
       * 0.6.
       */
      atre[i * k] = Math.max(
        atre[i * k],
        0.6 * Math.min(1, deadTimePoints / timeAndSlopeIndices[i * k].x));
      
      /*
       * Allow a dead_time error.
       * 000061_refold_false_adapter_02250513.004_75.616.pico wants 0.7.
       * 000345_refold_true_adapter_02260321.008_110.pico wants 1.76470588.
       */
      atre[i * k + 1] = Math.max(
        atre[i * k + 1],
        1.77 * Math.min(1 / 1.77, deadTimePoints / surfaceDelayPoints));
    }
    for (int i = Math.min(atre.length, allowedTimeRelativeError.size()); --i >= 0; ) {
      atre[i] = allowedTimeRelativeError.getDouble(i);
    }

    if (logger.isLoggable(Level.FINER)) {
      logger.log(Level.FINER, "allowedTimeRelativeError={0}", allowedTimeRelativeError);
    }
    // 000060_02040339.011_lose lasts only 0.019 s and wants a lower correlation
    // bound of 0.97.
    double approachCorrelation = approachRampSizeM / approachVelocityMOverS > 0.05 ? 0.9932 : 0.95;
    double[] minR2 = new double[timeAndSlopeIndices.length];
    for (int i = 0; i < iterations; ++i) {
      /*
       * stripchart_refolding/*refold_false_adapter* want lower correlation in
       * the fast segment.
       */
      // 000166_refold_true_117.00000000000003_adapter_02280229.003.pico wants
      // 0.9954
      // 000021_refold_true_125.99999999999999_adapter_02280420.011.pico wants
      // 0.993
      // 000019_refold_true_125.99999999999999_adapter_02280420.009.pico wants
      // 0.9932
      minR2[i * k] = approachCorrelation;
      // For almost horizontal data, the variance in Y is very low and
      // it makes the correlation very small.
      minR2[i * k + 1] = 0.00;
      minR2[i * k + 2] = 0.99901;
      if (i != iterations - 1) {
        minR2[i * k + 3] = 0.0;
      }
    }
    CommonThings.squareInPlace(minR2);
    JunctionPointsWithCost jpc = matchSlopesWithDynProgramming(
      stripChart.sharedApproach().sharedZArray(), slopes, timeAndSlopeIndices,
      atre, minR2);
    return jpc == null ? null : IntLists.unmodifiable(IntArrayList.wrap(jpc.junctionPoints));
  }

  public static ApproachRetractionLoop createLoopGivenJunctionPoints(
    StripChartPicoData stripChart, double approachRampSizeM, double approachVelocityMOverS,
    double retractionRampSizeM, double retractionVelocityMOverS, int iterations,
    IntList junctionPoints) {
    final int k = ApproachRetractionLoop.APPROACH_RETRACTION_SEGMENTS + 1;
    PlainPicoData[] approachRetractionCycles = new PlainPicoData[iterations];
    for (int i = iterations; --i >= 0;) {
      int approachFrom = junctionPoints.getInt(i * k);
      // +1 because To is exclusive
      int approachTo = junctionPoints.getInt(i * k + 1) + 1;
      int retractionFrom = junctionPoints.getInt(i * k + 2);
      // +1 because To is exclusive
      int retractionTo = junctionPoints.getInt(i * k + 3) + 1;
      PlainPicoData plainPicoData = extractPlainPicoDataFromSegment(
        stripChart, approachRampSizeM, approachVelocityMOverS, retractionRampSizeM,
        retractionVelocityMOverS, approachFrom, approachTo, retractionFrom, retractionTo);
      approachRetractionCycles[i] = plainPicoData;
    }
    return new ApproachRetractionLoop(junctionPoints, approachRetractionCycles);
  }

  public static double speedInZLsbPerPoint(StripChartPicoData stripChart, double velocityMOverS) {
    return velocityMOverS / (stripChart.zLsbToMetresFactor * stripChart.stripChartRateHz);
  }

  public static PlainPicoData extractPlainPicoDataFromSegment(
    StripChartPicoData stripChart, double approachRampSizeM, double approachVelocity,
    double retractionRampSizeM, double retractionVelocity, int approachFrom, int approachTo,
    int retractionFrom, int retractionTo) {
    short[] approachDeflection = Arrays.copyOfRange(
      stripChart.sharedApproach().sharedDeflectionArray(), approachFrom, approachTo);
    short[] approachZ = Arrays.copyOfRange(
      stripChart.sharedApproach().sharedZArray(), approachFrom, approachTo);
    if (Double.isNaN(approachRampSizeM)) {
      approachRampSizeM = (approachZ[approachZ.length - 1] - approachZ[0]) * stripChart.zLsbToMetresFactor;
      if (approachRampSizeM < 0) {
        logger.log(Level.WARNING, "approachRampSizeNm={0}", approachRampSizeM * 1e9);
        retractionRampSizeM = 0;
      }
    }
    if (Double.isNaN(approachVelocity)) {
      approachVelocity = approachRampSizeM * stripChart.stripChartRateHz / approachZ.length;
    }
    ArrayUtils.reverse(approachDeflection);
    ArrayUtils.reverse(approachZ);
    PlainDirectionData plainApproach = new PlainDirectionData(
      approachDeflection, approachZ, approachVelocity);
    short[] retractionDeflection = Arrays.copyOfRange(
      stripChart.sharedApproach().sharedDeflectionArray(), retractionFrom, retractionTo);
    short[] retractionZ = Arrays.copyOfRange(
      stripChart.sharedApproach().sharedZArray(), retractionFrom, retractionTo);
    if (Double.isNaN(retractionRampSizeM)) {
      retractionRampSizeM = (retractionZ[0] - retractionZ[retractionZ.length - 1]) * stripChart.zLsbToMetresFactor;
      if (retractionRampSizeM < 0) {
        logger.log(Level.WARNING, "retractionRampSizeNm={0}", retractionRampSizeM * 1e9);
        retractionRampSizeM = 0;
      }
    }
    if (Double.isNaN(retractionVelocity)) {
      retractionVelocity = retractionRampSizeM * stripChart.stripChartRateHz / retractionZ.length;
    }
    PlainDirectionData plainRetraction = new PlainDirectionData(
      retractionDeflection, retractionZ, retractionVelocity);
    double rampSizeM = (approachRampSizeM + retractionRampSizeM) / 2;
    PlainPicoData plainPicoData = new PlainPicoData(
      stripChart.xOffsetM, stripChart.springConstantNPerM, stripChart.sensZSensMPerV,
      stripChart.zScanStartHardValueV, stripChart.zLsbToMetresFactor, stripChart.deflectionLimitV,
      stripChart.deflectionSensitivityMPerV, Double.NaN, plainApproach, plainRetraction,
      rampSizeM / stripChart.sensZSensMPerV, (retractionFrom + 1.0 - approachTo)
        / stripChart.stripChartRateHz);
    return plainPicoData;
  }

  static class PossibleSegment {
    int from, to;
    double sumSquaredErrors;

    public void setup(int from, int to, double sumSquaredErrors) {
      Preconditions.checkArgument(from + 1 < to);
      Preconditions.checkArgument(sumSquaredErrors >= 0);
      this.from = from;
      this.to = to;
      this.sumSquaredErrors = sumSquaredErrors;
    }
    
    public boolean isEmpty() {
      return from == -1;
    }
    
    public void empty() {
      from = to = -1;
      sumSquaredErrors = Double.NaN;
    }

    @Override
    public String toString() {
      return ReflectionToStringBuilder.toString(this, ToStringStyle.SHORT_PREFIX_STYLE);
    }

    public double cost(double d) {
      // A longer segment needs to correlate better to make it here.
      double a = Math.abs((to - from) - d);
      double b = Math.sqrt(sumSquaredErrors / (to - from)) * (d + a);
      return b;
    }
  }

  public static class JunctionPointsWithCost {
    public final int[] junctionPoints;
    public final double cost;

    public JunctionPointsWithCost(int[] junctionPoints, double cost) {
      this.junctionPoints = Preconditions.checkNotNull(junctionPoints);
      this.cost = cost;
    }

    @Override
    public int hashCode() {
      final int prime = 31;
      int result = 1;
      long temp;
      temp = Double.doubleToLongBits(cost);
      result = prime * result + (int) (temp ^ (temp >>> 32));
      result = prime * result + Arrays.hashCode(junctionPoints);
      return result;
    }

    @Override
    public boolean equals(Object obj) {
      if (this == obj)
        return true;
      if (obj == null)
        return false;
      if (getClass() != obj.getClass())
        return false;
      JunctionPointsWithCost other = (JunctionPointsWithCost) obj;
      if (Double.doubleToLongBits(cost) != Double.doubleToLongBits(other.cost))
        return false;
      if (!Arrays.equals(junctionPoints, other.junctionPoints))
        return false;
      return true;
    }

    @Override
    public String toString() {
      return ReflectionToStringBuilder.toString(this, ToStringStyle.SHORT_PREFIX_STYLE);
    }
  }

  /**
   * @param a
   * @param timeStep
   *          NOT the time between two successive points. Instead, considering a
   *          point to be the unit of time, it represents the number of points
   *          that should be considered as a window to categorise based on the
   *          slope.
   * @return the initial position and then the position where each of the
   *         patterns ends
   */
  public static int[] matchSlopesWithRegex(
    short[] a, double[] slopes, DoubleIntPair[] timeAndSlopeIndices, double timeStep) {
    logger.log(
      Level.INFO, "a.length={0} slopes={1} timeAndSlopeIndices={2} timeStep={3}", new Object[] {
      a.length, DoubleArrayList.wrap(slopes), Arrays.asList(timeAndSlopeIndices), timeStep});
    for (DoubleIntPair timeAndSlopeIndex : timeAndSlopeIndices) {
      Preconditions.checkArgument(
        0 <= timeAndSlopeIndex.y && timeAndSlopeIndex.y < slopes.length, timeAndSlopeIndex);
    }
    double[] time = DoubleIntPair.x(timeAndSlopeIndices);
    // double totalTime = CommonThings.sum(time);
    // Preconditions.checkArgument(totalTime <= a.length - 1,
    // "data is not long enough");
    Preconditions.checkArgument(timeStep >= 2, "at least 2 points are required for a segment");
    Preconditions.checkArgument(
      timeStep <= CommonThings.min(time) / 2, "TimeStep shold be at most %s but it is %s.",
      CommonThings.min(time) / 2, timeStep);
    DoubleIntPair[] sortedAngleAndIndices = sortedAnglesAndIndices(slopes);

    double[] actualAngles = new double[(int) Math.ceil(a.length / timeStep) - 1];
    LongArrayRegression regression = LongArrayRegression.create(a);
    for (int i = 0; i < actualAngles.length; ++i) {
      int start = (int) Math.ceil(i * timeStep), end = (int) Math.ceil((i + 1) * timeStep);
      // end[i] == start[i + 1]
      assert end - start >= 1;
      actualAngles[i] = TAN ? regression.slope(start, end + 1) : atan2(regression.sxy(start, end + 1), regression.sxx(start, end + 1));
    }
    int[] classification = new int[actualAngles.length];
    double[] sortedAngles = DoubleIntPair.x(sortedAngleAndIndices);
    Preconditions.checkArgument(CommonThings.increasing(sortedAngles), "slopes are not distinct");
    for (int i = actualAngles.length; --i >= 0;) {
      classification[i] = sortedAngleAndIndices[CommonThings.indexOfNearestHalfEvenInSortedArray(
        actualAngles[i], sortedAngles)].y;
    }
    logger.log(
      Level.INFO,
      "sortedAngles={0} actualAngles={1} classification={2}",
      new Object[] {
      DoubleArrayList.wrap(sortedAngles), DoubleArrayList.wrap(actualAngles),
      IntArrayList.wrap(classification)});
    // Here comes the messy part.
    Preconditions.checkArgument(
      slopes.length <= 'z' - 'a', "Currently only %s slopes are supported.", 'z' - 'a');
    char[] seqChars = new char[classification.length];
    for (int i = seqChars.length; --i >= 0;) {
      seqChars[i] = toChar(classification[i]);
    }
    String seq = String.valueOf(seqChars);
    StringBuilder regex = new StringBuilder();
    for (int i = 0; i < timeAndSlopeIndices.length; ++i) {
      char c = toChar(timeAndSlopeIndices[i].y);
      double idealCount = timeAndSlopeIndices[i].x / timeStep;
      assert idealCount >= 2;
      logger.log(Level.FINER, "idealCount={0}", idealCount);
      String prefix = i == 0 ? "" : String.format("[^%c]??", c);
      String suffix = i == timeAndSlopeIndices.length - 1 ? "" : String.format("[^%c]??", c);
      regex.append(String.format(
        "(%1$s%2$c{%3$d,%4$d}+%5$s)", prefix, c, (int) Math.floor(idealCount * 0.80),
        (int) Math.ceil(idealCount * 1.20), suffix));
    }
    logger.log(Level.INFO, "regex={0} seq={1}", new Object[] {regex, seq});
    /*
     * TODO: Python returns a negative result very quickly with deadTime / 4,
     * but Java hangs on forever. Check if other Java regex libraries (such as
     * the one based on an FSA) are faster. TODO: precompile and cache?
     */
    Pattern pattern = Pattern.compile(regex.toString());
    // jregex.Pattern pattern = new jregex.Pattern(regex.toString());
    logger.info("Regex matching.");
    // TODO: try also jregex
    Matcher matcher = pattern.matcher(seq);
    // jregex.Matcher matcher = pattern.matcher(seq);
    boolean found = matcher.find();
    if (!found) {
      logger.info("Pattern not found.");
      return null;
    }
    assert matcher.groupCount() == timeAndSlopeIndices.length;
    // assert matcher.groupCount() == 1 + timeAndSlopeIndices.length;
    int[] result = new int[matcher.groupCount() + 1];
    for (int i = 1; i <= matcher.groupCount(); ++i) {
      if (i != matcher.groupCount()) {
        assert matcher.end(i) == matcher.start(i + 1);
      }
      result[i - 1] = (int) Math.ceil(matcher.start(i) * timeStep);
    }
    result[matcher.groupCount()] = (int) Math.ceil(matcher.end(matcher.groupCount()) * timeStep);
    return result;
  }

  private static double atan2(double y, double x) {
    /*
     * Check "Efficient approximations for the arctangent function".
     * (Math.PI / 4 - (absYOverX - 1) * (0.2447 + 0.0663 * absYOverX)) * yOverX;
     */
    return Math.atan2(y, x);
  }

  private static DoubleIntPair[] sortedAnglesAndIndices(double[] slopes) {
    DoubleIntPair[] sortedAngleAndIndices = new DoubleIntPair[slopes.length];
    for (int i = slopes.length; --i >= 0;) {
      sortedAngleAndIndices[i] = new DoubleIntPair(TAN ? slopes[i] : Math.atan(slopes[i]), i);
    }
    Arrays.sort(sortedAngleAndIndices);
    return sortedAngleAndIndices;
  }

  /**
   * @param a
   * @param slopes
   * @param matches
   * @param fuzziness
   *          ~timeStep
   * @param lineFitWindowLength
   *          ~timeStep
   * @return
   */
  public static int[] improveSlopeMatches(
    short[] a, double[] slopes, int[] matches, int fuzziness, int lineFitPoints) {
    Preconditions.checkArgument(lineFitPoints >= 2, lineFitPoints);
    Preconditions.checkArgument(slopes.length + 1 == matches.length);
    double[] angles = new double[slopes.length];
    for (int i = 0; i < slopes.length; ++i) {
      angles[i] = Math.atan(slopes[i]);
    }
    slopes = null;
    int[] result = new int[matches.length];
    double[] leftAngles = new double[fuzziness + 1 + fuzziness];
    double[] rightAngles = new double[leftAngles.length];
    double[] absError = new double[leftAngles.length];
    LongArrayRegression regression = LongArrayRegression.create(a);
    for (int i = 0; i < matches.length; ++i) {
      Arrays.fill(leftAngles, Double.NaN);
      Arrays.fill(rightAngles, Double.NaN);
      for (int j = matches[i] - fuzziness; j <= matches[i] + fuzziness; ++j) {
        if (j >= lineFitPoints - 1 && j < a.length && (i == 0 || j > matches[i - 1])) {
          leftAngles[j - (matches[i] - fuzziness)] = Math.atan2(regression.sxy(j - lineFitPoints + 1, j + 1), regression.sxx(j - lineFitPoints + 1, j + 1));
        }
        if (j >= 0 && j <= a.length - lineFitPoints
          && (i == matches.length - 1 || j < matches[i + 1])) {
          rightAngles[j - (matches[i] - fuzziness)] = Math.atan2(regression.sxy(j, j + lineFitPoints), regression.sxx(j, j + lineFitPoints));
        }
      }
      if (i == 0) {
        for (int j = 0; j < absError.length; ++j) {
          absError[j] = Math.abs(rightAngles[j] - angles[0]);
        }
      } else if (i == matches.length - 1) {
        for (int j = 0; j < absError.length; ++j) {
          absError[j] = Math.abs(leftAngles[j] - angles[i - 1]);
        }
      } else {
        for (int j = 0; j < absError.length; ++j) {
          if (Double.isNaN(leftAngles[j])) {
            absError[j] = 2 * Math.abs(rightAngles[j] - angles[i]);
          } else if (Double.isNaN(rightAngles[j])) {
            absError[j] = 2 * Math.abs(leftAngles[j] - angles[i - 1]);
          } else {
            absError[j] = Math.abs(leftAngles[j] - angles[i - 1])
              + Math.abs(rightAngles[j] - angles[i]);
          }
        }
      }
      int argMin = CommonThings.argMinNotNaNFirst(absError);
      if (argMin == -1) {
        result[i] = matches[i];
      } else {
        int index;
        if (i == 0) {
          index = argMin;
        } else if (i == matches.length - 1) {
          index = ArrayUtils.lastIndexOf(absError, absError[argMin]);
        } else {
          index = CommonThings.middleIndexOf(absError, absError[argMin]);
        }
        result[i] = matches[i] - fuzziness + index;
      }
    }
    return result;
  }

  private static char toChar(int index) {
    Preconditions.checkArgument(0 <= index && index <= 'z' - 'a', "index=%s", index);
    return (char) ('a' + index);
  }
  
  private static final boolean TAN = true;
  
  public static JunctionPointsWithCost matchSlopesWithDynProgramming(
    short[] z, double[] slopes, DoubleIntPair[] timeAndSlopeIndices,
    double[] allowedTimeRelativeError, double[] minR2) {
    for (DoubleIntPair timeAndSlopeIndex : timeAndSlopeIndices) {
      Preconditions.checkArgument(timeAndSlopeIndex.x >= 0);
      Preconditions.checkArgument(timeAndSlopeIndex.y >= -1);
    }
    final int n = timeAndSlopeIndices.length;
    Preconditions.checkArgument(minR2.length == n);
    Preconditions.checkArgument(allowedTimeRelativeError.length == n);

    DoubleIntPair[] sortedAngleAndIndices = sortedAnglesAndIndices(slopes);
    final double[] sortedAngles = DoubleIntPair.x(sortedAngleAndIndices);
    Preconditions.checkArgument(CommonThings.firstNonIncreasingIndex(sortedAngles) == sortedAngles.length, sortedAngles);
    
    int[] slopeIndexInverses = new int[sortedAngleAndIndices.length];
    Arrays.fill(slopeIndexInverses, -1);
    for (int i = 0; i < sortedAngleAndIndices.length; ++i) {
      assert slopeIndexInverses[sortedAngleAndIndices[i].y] == -1;
      slopeIndexInverses[sortedAngleAndIndices[i].y] = i;
    }
    sortedAngleAndIndices = null;  // Not used any longer.
    int[][] from = new int[n][z.length];
    LongArrayRegression regression = LongArrayRegression.create(z);
    int[] minLens = new int[n];
    for (int i = n; --i >= 0;) {
      minLens[i] = Math.max(
        2, (int) Math.ceil(timeAndSlopeIndices[i].x * (1 - allowedTimeRelativeError[i])));
    }
    int[] occupied = new int[n];
    occupied[n - 1] = minLens[n - 1] - 1;
    for (int i = n - 1; --i >= 0;) {
      occupied[i] += minLens[i] - 1;
    }
    // prev must be all-zeroes.
    double[] prev = new double[z.length];
    double[] current = new double[z.length];
    PossibleSegment ps = new PossibleSegment();
    for (int i = 0; i < n; ++i) {
      double time = timeAndSlopeIndices[i].x;
      int slopeIndexInverse = timeAndSlopeIndices[i].y == -1 ? -1 : slopeIndexInverses[timeAndSlopeIndices[i].y];
      double[] tmp = prev;
      prev = current;
      current = tmp;
      Arrays.fill(current, Double.POSITIVE_INFINITY);
      // Optimisation 1 that helps reduce the time by 1/7.
      int lowestValidLastPreviousIteration = 0;
      if (i != 0) {
        while (lowestValidLastPreviousIteration != z.length
          && prev[lowestValidLastPreviousIteration] == Double.POSITIVE_INFINITY) {
          ++lowestValidLastPreviousIteration;
        }
      }
      int highestValidLastPreviousIteration = z.length - 1;
      if (i != 0) {
        while (highestValidLastPreviousIteration != -1
          && prev[highestValidLastPreviousIteration] == Double.POSITIVE_INFINITY) {
          --highestValidLastPreviousIteration;
        }
      }
      final int minLen = minLens[i];
      // Optimisation 2 that helps reduce the time by very little. Perhaps
      // not worth it.
      int accessibleLength = z.length - (i == n - 1 ? 0 : occupied[i + 1]);
      final int maxLen = Math.min(
        accessibleLength - lowestValidLastPreviousIteration,
        (int) Math.floor(time * (1 + allowedTimeRelativeError[i])));
      for (int len = maxLen + 1; --len >= minLen;) {
        // Optimisation 3 that reduces the execution time by like 50%.
        int reallyAccessibleLength = Math.min(accessibleLength, highestValidLastPreviousIteration
          + len);
        if (prev[lowestValidLastPreviousIteration] < current[lowestValidLastPreviousIteration
            + len - 1]) {
          perhapsCreatePossibleSegment(
            minR2, sortedAngles, regression, i, slopeIndexInverse,
            lowestValidLastPreviousIteration, lowestValidLastPreviousIteration + len, ps);
          if (!ps.isEmpty()) {
            double potentialCost = prev[lowestValidLastPreviousIteration] + ps.cost(time);
            if (potentialCost < current[lowestValidLastPreviousIteration + len - 1]) {
              current[lowestValidLastPreviousIteration + len - 1] = potentialCost;
              from[i][lowestValidLastPreviousIteration + len - 1] = ps.from;
            }
          }
        }
        for (int last = lowestValidLastPreviousIteration + len; last < reallyAccessibleLength; ++last) {
          if (prev[last - (len - 1)] < current[last]) {
            perhapsCreatePossibleSegment(
              minR2, sortedAngles, regression, i, slopeIndexInverse,
              last + 1 - len, last + 1, ps);
            if (!ps.isEmpty()) {
              double potentialCost = prev[last - (len - 1)] + ps.cost(time);
              if (potentialCost < current[last]) {
                current[last] = potentialCost;
                from[i][last] = ps.from;
              }
            }
          }
        }
      }
    }
    // Expand the last segment as much as possible if the cost is minimum.
    int argMin = CommonThings.lastArgMin(current, 0, current.length);
    final int[] result;
    if (current[argMin] == Double.POSITIVE_INFINITY) {
      logger.warning("Could not find any valid path.");
      result = null;
    } else {
      result = new int[n + 1];
      int k = argMin;
      for (int i = n; --i >= 0;) {
        result[i + 1] = k;
        k = from[i][k];
      }
      result[0] = k;
    }
    Level level = Level.FINER;
    if (logger.isLoggable(level) & result != null) {
      int[] optimalTime = new int[n];
      for (int i = 0; i < n; ++i) {
        optimalTime[i] = result[i + 1] - result[i] + 1;
      }
      double[] r2 = new double[n];
      for (int i = n; --i >= 0;) {
        r2[i] = regression.rSquare(result[i], result[i + 1] + 1);
      }
      logger.log(level, "DesiredTimes={0} OptimalTimes={1} SquareCorrelations={2}", new Object[] {
      DoubleArrayList.wrap(DoubleIntPair.x(timeAndSlopeIndices)), IntArrayList.wrap(optimalTime),
      DoubleArrayList.wrap(r2)});
    }
    return result == null ? null : new JunctionPointsWithCost(result, current[argMin]);
  }
  
  private static void perhapsCreatePossibleSegment(
    double[] minR2, double[] sortedAngles,
    LongArrayRegression regression, int segment, int slopeIndexInverse, int from, int to, PossibleSegment ps) {
    int n = to - from;
    long sumX = regression.sumX(from, to);
    long sumY = regression.sumY(from, to);
    long sumXy = regression.sumXy(from, to);
    long sumX2 = regression.sumX2(from, to);
    long sumY2 = regression.sumY2(from, to);
    long sxx = LongArrayRegression.sxx(n, sumX, sumX2);
    long sxy = LongArrayRegression.sxy(n, sumX, sumY, sumXy);
    long syy = LongArrayRegression.syy(n, sumY, sumY2);
    double r2 = LongArrayRegression.rSquare(sxx, sxy, syy);
    if (r2 >= minR2[segment] | (Double.isNaN(r2) & minR2[segment] == 0)) {
      double angle = TAN ? LongArrayRegression.slope(sxy, sxx) : atan2(sxy, sxx);
      final boolean ok;
      if (slopeIndexInverse == -1) {
        ok = true;
      } else {
        double diff = Math.abs(sortedAngles[slopeIndexInverse] - angle);
        ok = (slopeIndexInverse == sortedAngles.length - 1 || diff <= sortedAngles[slopeIndexInverse + 1] - angle)
          &
          ((slopeIndexInverse == 0 || diff <= angle - sortedAngles[slopeIndexInverse - 1]));
      }
      if (ok) {
        double sse = LongArrayRegression.sumOfSquaredErrors(n, sxx, sxy, syy);
        double sumSquaredErrors = Double.isNaN(sse) ? Double.POSITIVE_INFINITY : sse;
        ps.setup(from, to, sumSquaredErrors);
      } else {
        ps.empty();
      }
    } else {
      ps.empty();
    }
  }

  public static IntList computeLoopJunctionPointsOrDefault(
    StripChartPicoData stripchart, int i, int timeInstantCount, LoopParams loopParams,
    DoubleList allowedTimeRelativeError) {
    IntList jp = computeLoopJunctionPoints(
      stripchart, 0, stripchart.timeInstantCount(), loopParams, allowedTimeRelativeError);
    if (jp == null) {
      jp = JpHelper.createDefaultJunctionPoints(loopParams.iterations
        * (ApproachRetractionLoop.APPROACH_RETRACTION_SEGMENTS + 1));
    }
    return jp;
  }
}
